#include <stdio.h>
#include <string.h>
#include <math.h>
#include <unistd.h>
#include <assert.h>

/*========================================================================
  This is a version of the facenorm.c program originally used to 
  normalize FERET face images for the FERET 1996/97 studies. This code
  was originally used by Moon and Phillips to pre-process imagery for
  the FERET 1996/97 tests. It was subsequently updated by Patric
  Grother at NIST.  float *data, float *data, float *data,

  At CSU we have disentangled this source code from the larger 
  FERET code distribution. We have left many of the previous comments
  in place, although some are no longer accurate. 

  Here is updated usage and compilation information. 

  Usage: csu1face2norm [options] coordinates_file images_in_dir images_out_dir
  Options:
         -v verbose
         -h turn off histogram equalization
         -r turn off rotation normalization
         -s turn off standardization to zero mean and unit standard deviation
         -p write a preview of transformed image in pgm format
         -m maskfile.dat
                                 use "0" to not apply a mask
                                 with no option the default mask is m150x130.dat

  Compile: 
    gcc  -I. -I/usr/include  -L/usr/lib -lm  csu1face2norm.c   -o csu1face2norm

  The changes we have made include:
  1. We have shifted this code back to being ANSI C with no C++. There
     was indeed no real use of C++ in the sense of being object
     oriented. The primary shift to get the code back to straight C was to
     shift from new and delete back to malloc and free.

  2. The input images are now read from pgm formatted images. This is
     much simpler than using JPEG, and the jpeg images that come with
     the FERET distribution are indeed functionally near equivalent to 
     PGM, with the exeception that coding a reader is non-trivial. When
     the same two images are stored to disk in jpeg and pgm, the pgm is 
     actually slightly smaller.

  3. Added an optional facility to write a preview version of the raw
     data image in PGM format for easy viewing. do NOT use these as
     replacements for the actual "*.nrm" images generated by this
     normilization code.

  Ross Beveridge                        May 7, 2001
************************************************************************/

// Prior doc header from NIST.
/**************************************************************
   facenorm.c : program to rotate the face to horizontal,
		based on the (reye+leye)/2 and mouth coordinates 
		and then rescale & crop to a fixed size.
	    
   usage : facenorm
          filelist contains the listing of the files,
	  plus the eye coordinates
          
   input : fileinfo.dat

   compile : gcc facenorm.c byte_swap.c -o face2 -lm -O4

   Modified on: 5/11/98	Hyeonjoon Moon:
			unspecifed modifications
                x/11/99 Jonathon Phillips:
			modifed to run on Windows NT using Cygwin B20
		1/20/00	Patrick Grother:
			modified to send error messages to stderr
			modified to detect failed sscanf calls
			removed leaks in resize
			removed unneeded mallocs before while loop
			added calls to free where necessary
		1/24/00 Patrick Grother:
			replaced calloc by malloc in hist_equal where
                        possible

-----------------------------------------------------------------

   renamed Jan 30 1999 (the previous face_NT_Hist remain in a sister dir)
   facenorm : function is the same, only the image input are
              expected to be standard JPEG files.

   Modified on	1/30/00 Patrick Grother:
 			replaced old plain byte reading code with
			call to jpeg reader in library

		1/30/00	removed large character arrays for strings
			replaced #define with typed globals
			put masking read and ops in subroutines

                2/12/00 fixed histogram bug

               10/25/00 added command line switches for histogram equalization
			and intensity standardization
               
****************************************************************/

/*====================================================================*/
/* Begin Code                                                         */
/*====================================================================*/

typedef unsigned char UCH;


static int min(const int a, const int b) { return (a < b) ? a : b; }

/*------------------------------------------------------------------------
  Function Prototypes
  ------------------------------------------------------------------------*/

float *readferetraster(const char *fn,int numpix);
float *writeferetraster(const char *fn, float *data, int numpix);

UCH *readImagePGM(const char *filename, int *w, int *h, int verbose);
void writeImagePGM(const char *fn, float *data, int numpix, int w, int h);

int isMachineLittleEndian();

void byteswap_4(void *data, const int numfourbyteelements);

char **read_strings(const char *fn, int *numlines);
void free_strings(char **x, const int n);

FILE *fopen_with_error(const char *fn, const char *mode, const char *routine);
FILE *fopen_anycase(const char *fn, const char *mode, const char *routine);

void strnullchecks(const char *fn, const char *name, const char *routine);
char *strconc(const char *s1, const char *s2);
char *strclone(const char *si);
char *strlower(char *x);
char *strupper(char *x);
char *newextlong(char **filename, const char *extension);

static void bailer(const char *s1, const char *s2, const char *s3);
void syserr(const char *funcname, const char *syscall, const char *msg);



/*------------------------------------------------------------------------
  Utilities integrated directly into this source from elsewhere in FERET
  code distribution.
  ------------------------------------------------------------------------*/

float *readferetraster(const char *fn,  int numpix)
{
   const char *routine = "readferetraster";
   FILE *fp;
   float* data;

   fp = (FILE*) fopen_with_error(fn, "rb", routine);
   
   data = (float*)malloc(sizeof(float)*numpix);
   
   if(!data){ 
     syserr("readferetraster", "malloc of data failed", "");
   }
     

   if (!fread(data, sizeof(float), numpix, fp))
      syserr(routine, fn, "fread");

   if (isMachineLittleEndian()) byteswap_4(data, numpix);
   
   fclose(fp);
   return data;
}



float *writeferetraster(const char *fn, float *data, int numpix)
{
   const char *routine = "writeferetraster";
   FILE *fp;

   fp = (FILE*) fopen_with_error(fn, "wb", routine);
   
   if (isMachineLittleEndian()) byteswap_4(data, numpix);
   
   if (numpix != fwrite(data, sizeof(float), numpix, fp))
      syserr(routine, fn, "fwrite");

   fclose(fp);
   return data;
}

int isMachineLittleEndian() {
  char magic[4] = {0x01,0x02,0x03,0x04};
  unsigned long *longMagic = (unsigned long *) magic;
  if (*longMagic == 0x01020304) return 0;
  if (*longMagic == 0x04030201) return 1;
  fprintf(stderr,"Funky Byte Order, I give Up!!\n");
  exit(0);
}

void byteswap_4(void *data, const int numfourbyteelements)
{
  int i;
  int *d4 = (int *)data;
  UCH *d1 = (UCH *)data;

   if (!(isMachineLittleEndian()))	// i.e. BIG_ENDIAN
   {
      for (i = 0 ; i < numfourbyteelements ; i++, d1 += 4 )
         d4[i] = ((int)d1[3] << 24) | ((int)d1[2] << 16) |
                 ((int)d1[1] <<  8) |  (int)d1[0];
   }
   else				// i.e. LITTLE_ENDIAN
   {
      for (i = 0 ; i < numfourbyteelements ; i++, d1 += 4 )
         d4[i] = ((int)d1[0] << 24) | ((int)d1[1] << 16) |
                 ((int)d1[2] <<  8) |  (int)d1[3];
   }
}

UCH *readImagePGM(const char *filename, int *w, int *h, int verbose)
{
   const char *routine = "readImagePGM()";
   int  width, height, max, i, sum;
   int  val;
   char fchar;
   char line[100];
   char ftype[16];
   FILE *infile;
   UCH *im;

   /* Read first line, and test if it is propoer PNM type */
   if (verbose) fprintf(stdout,"Going to open file %s\n", filename); 
   infile = (FILE*) fopen_with_error(filename, "rb", routine);
   fgets(line,100,infile);
   sscanf(line," %s",&ftype);
   if (verbose) fprintf(stdout,"File Type is %s.\n", ftype);
   if (! (strcmp(ftype,"P5") == 0)) {
     syserr("face2norm", "Currenlty only binary pgm files, type P5, supported", "");
   }

   /* Read lines, ignoring those starting with Comment Character, until the
      Image Dimensions are read. */
   fchar = '#';
   while (fchar == '#') {
     fgets(line,100,infile);
     sscanf(line, " %c", &fchar);
   }
   if (verbose) fprintf(stdout,"Second non-comment line of image file %s.\n", line);
   sscanf(line, " %d %d", &width, &height);
   *w = width;
   *h = height;
   if (verbose) fprintf(stdout,"The width,  height and size are: %d %d %d\n", width, height, width * height);

   /* Read lines, ignoring those starting with Comment Character, until the
      maximum pixel value is read. */
   fchar = '#';
   while (fchar == '#') {
     fgets(line,100,infile);
     sscanf(line, " %c", &fchar);
   }
   sscanf(line, "%d", &max);
   if (verbose) fprintf(stdout,"The max value for the pixels is: %d\n", max);
   if (! (max == 255)) {
     fprintf(stdout,"readImagePGM: Warning, max value %d for pixels in image %s is not 255\n", max, filename); 
   }

   im = (UCH*) malloc(sizeof(UCH)*width*height);
   if (!im) syserr("readImagePGM", "malloc of im failed", "");

   i = 0;
   val = fgetc(infile);
   while (!(val == EOF)) {
     im[i] = (UCH) val;
     i++;
     val = fgetc(infile);
   }
   if (verbose) fprintf(stdout,"Read in %d Pixel Values\n", i);
   fclose(infile);
   return( im );
}
   

void writeImagePGM(const char *fn, float *data, int numpix,int w, int h)
{
   const char *routine = "writeImagePGM";
   float min, max, sum, scale;
   int  i, val;
   FILE *fp;

   min = data[0];
   max = data[0];
   sum = 0.0;

   for (i = 1; i < numpix; i++) {
     if (data[i] < min) min = data[i];
     if (data[i] > max) max = data[i];
     sum = sum + data[i];
   }

   scale = 255.0 / (max - min);

   fp = (FILE*) fopen_with_error(fn, "wb", routine);
   
   fprintf(fp,"P5\n");
   fprintf(fp,"%d %d\n", w, h);
   fprintf(fp,"255\n");
   for (i = 0; i < numpix; i++) {
     val = (int) ((data[i] - min) * scale);
     fputc(val,fp);
   }
   fclose(fp);
}

/*========================================================================
  Contents of what was the readstrs.c file in NIST library
  ========================================================================*/

char **read_strings(const char *fn, int *numlines)
{
   const char *routine = "read_strings";
   char aline[512];
   char **values    = 0;
   char **newvalues = 0;
   int i; 
   int numread  = 0;
   int numalloc = 0;
   FILE *fp;

   fp = (FILE*) fopen_with_error(fn, "r", routine);
   while (fgets(aline, 512, fp) != NULL)
   {
      if (numread == numalloc)
      {
         numalloc = 16 + 11*numalloc/10;
         newvalues = (char**) malloc(sizeof(char *)*numalloc);
         if (!newvalues)
            exit(fprintf(stderr, "%s: new char ** [%d] failed\n", routine, numalloc));
      
         for ( i = 0       ; i < numread  ; i++ )  
	   newvalues[i] = values[i];
         for ( i = numread ; i < numalloc ; i++ )
	   newvalues[i] = 0;
	 if(values!=NULL)
	   free(values);
         values = newvalues;
      }

      {
         char *linereturn = strrchr(aline, '\n');
         if (linereturn)
            *linereturn = '\0'; /* chomp */
      }
      values[numread++] = strclone(aline);
   }
   fclose(fp);
   *numlines = numread;
   return values;
}

void free_strings(char **x, const int n)
{
  int i; 
   for ( i = 0 ; i < n ; i++ )
      free(x[i]);
   free(x);
}


// make a stand alone concatenation of s1+s2
char *strconc(const char *s1, const char *s2)
{
  int n1, n2, nreq;
  char *result;

   n1   = strlen(s1);
   n2   = strlen(s2);
   nreq = n1+n2+1;

   result = (char*) malloc(sizeof(char)*nreq);
   if (!result)
         syserr("strconc", "new char []", "space for string");

   memcpy(&result[0 ], s1, n1);
   memcpy(&result[n1], s2, n2);
   result[n1+n2] = '\0';
   return result;
}

char *strclone(const char *si)
{
  int ni;
  char *so;


  ni = strlen(si);
  so = (char*) malloc(sizeof(char)*(ni+1));
  if (!so) syserr("strclone", "malloc for string ", "failed");

  memcpy(so, si, ni);
  so[ni] = '\0';
  return so;
}

void strnullchecks(const char *fn, const char *name, const char *routine)
{
   if (fn == NULL)
   {
      fprintf(stderr, "routine \"%s\" was supplied with NULL variable \"%s\"\n",
         routine, name);
      exit(-1);
   }
}

// make all the characters of the input string lower case, if applicable
// changes the input string
char *strlower(char *x)
{
   char *result;
   result = x ;
   strnullchecks(x, "x", "strlower");
   for ( ; *x ; x++ )
      *x = (char)tolower((int)*x);
   return result;
}


// make all the characters of the input string lower case, if applicable
// changes the input string
char *strupper(char *x)
{
   char *result;
   result = x ;
   strnullchecks(x, "x", "strupper");
   for ( ; *x ; x++ )
      *x = (char)toupper((int)*x);
   return result;
}


// take two strings: strip the final extension from the first
// if any then add the second to the first.
// note the first string's address is altered, it's memory is
// freed and it points to newly allocated memory afterwards.
char *newextlong(char **filename, const char *extension)
{
   if (extension)
   {
      char *result = 0;
      char *finalperiod = strrchr(*filename, '.');

      if (!finalperiod)	// add an extension where there wasn't one
      {
         char *periodized = strconc(".", extension);
         result = strconc(*filename, periodized);
         free(periodized);
      }
      else
      {				// replace the existing extension
         finalperiod[1] = '\0';	// NULL terminate the string
         result = strconc(*filename, extension);
      }

      free(*filename);
      *filename = result;
   }
   return *filename;
}



static void bailer(const char *s1, const char *s2, const char *s3)
{
  fprintf(stderr,"ERROR");
  if (s1) fprintf(stderr,": %s", s1);
  if (s2) fprintf(stderr,": %s", s2);
  if (s3) fprintf(stderr,": %s", s3);
  fprintf(stderr,"\n");
  fflush(stderr);
}

void syserr(const char *funcname, const char *syscall, const char *msg)
{
   bailer(funcname, syscall, msg);
   exit(-1);
}

FILE *fopen_with_error(const char *fn, const char *mode, const char *routine)
{
  FILE *fp;

  // for simplicity try the filename as it came in
  fp = fopen(fn, mode);
  if (fp != NULL) return fp;
  
  // then try different case variations, failing if that doesn't work
  return fopen_anycase(fn, mode, routine);
}

FILE *fopen_anycase(const char *fn, const char *mode, const char *routine)
{
  FILE *fp, *lp, *up;
  char *lower, *upper;

   // for simplicity try the filename as it came in
   fp = fopen(fn, mode);
   if (fp != NULL) return fp;

   // try the filename (including any path) all forced to lower case
   lower = strlower(strclone(fn));
   lp = fopen(lower, mode); free(lower);
   if (lp != NULL) return lp;

   // try the filename (including any path) all forced to upper case
   upper = strupper(strclone(fn));
   up = fopen(upper, mode); free(upper);
   if (up != NULL) return up;

   // else bail totally unless 
   {
     fprintf(stderr, "%s: cannot open(mode \"%s\") file %s\n",
	     routine ? routine : "fopen_anycase", mode, fn);
     exit(-1);
   }
}


